#include "Main.h"
#include "TestClass.h"
Main::Main()
{
#if 0
	// DRIVER CONTROL INSTANTIATIONS
	m_joystickDriver1 = new Joystick( JOYSTICK_DRIVER_1_PORT );
	m_joystickDriver2 = new Joystick( JOYSTICK_DRIVER_2_PORT );
	m_joystickOperator1 = new Joystick( JOYSTICK_OPERATOR_1_PORT );
	m_joystickOperator2 = new Joystick( JOYSTICK_OPERATOR_2_PORT );
	
	// SENSOR INSTANTIATIONS
	m_globals = GlobalData::GetInstance();
	m_encoderLeft = new Encoder( LEFT_ENCODER_A_DI, LEFT_ENCODER_B_DI );
	m_encoderRight = new Encoder( RIGHT_ENCODER_A_DI, RIGHT_ENCODER_B_DI );
	//m_gyro = new Gyro( GYRO_AI );
	m_launcherEMF = new AnalogChannel( MOTOR_EMF_AI );
	
	m_encoderLeft->Start();
	m_encoderRight->Start();
	
	// ACTUATOR INSTANTIATIONS
	// Drive train
	m_DTLeftA = new Talon( DRIVETRAIN_L_1_PWM );
	m_DTLeftB = new Talon( DRIVETRAIN_L_2_PWM );
	m_DTRightA = new Talon( DRIVETRAIN_R_1_PWM );
	m_DTRightB = new Talon( DRIVETRAIN_R_2_PWM );
	// Spatula
	m_spatula = new Solenoid( SOLENOID_SPATULA );
	// Hang
	m_hangA = new Solenoid( SOLENOID_HANG_A );
	m_hangB = new Solenoid( SOLENOID_HANG_B );
	// Hopper
	m_hopperContainer = new Solenoid( SOLENOID_HOPPER_CONTAINER );
	m_hopperDisc = new Solenoid( SOLENOID_HOPPER_DISC );
	m_hopperKicker = new Solenoid( SOLENOID_HOPPER_KICKER );
	// Launcher
	m_launcherLaunch = new Talon( LAUNCHER_PWM );
	m_launcherTilt = new Talon( TILT_PWM );
#endif
}
Main::~Main()
{
	//delete m_globals;
}

void Main::Autonomous()
{
	/*
	SevenAutonomous();
	Process();
	Output();
	*/
}
void Main::OperatorControl()
{
	/*
	Input();
	Process();
	Output();
	*/
}
void Main::Disabled()
{
	
}

void Main::Input()
{
#if 0
	// RECEIVE DATA FROM SENSORS
	m_globals->i_encoderLeft = m_encoderLeft->Get();
	m_globals->i_encoderRight = m_encoderRight->Get();
	//m_globals->i_gyroAngle = m_gyro->GetAngle();
	m_globals->i_hopperContainerState = m_hopperContainer->Get();
	m_globals->i_hopperDiscState = m_hopperDisc->Get();
	m_globals->i_hopperKickerState = m_hopperKicker->Get();
	m_globals->i_launcherSpeedBackEMF = m_launcherEMF->GetValue();
	
	
	// RECEIVE DATA FROM DRIVER CONTROLS
	// Drivetrain
	m_globals->i_joystick_1_x = m_joystickDriver1->GetX();
	m_globals->i_joystick_1_y = m_joystickDriver1->GetY();
	m_globals->i_joystick_2_x = m_joystickDriver2->GetX();
	m_globals->i_joystick_2_y = m_joystickDriver2->GetY();
	m_globals->i_joystickArcadeToggle = m_joystickDriver1->GetRawButton( JOYSTICK_ARCADE_TOGGLE );
	// Spatula
	m_globals->i_spatulaUp = m_joystickDriver2->GetRawButton( SPATULA_UP );
	m_globals->i_spatulaDown = m_joystickDriver2->GetRawButton( SPATULA_DOWN );
	// Hang
	m_globals->i_hangExtended = m_joystickDriver2->GetRawButton( HANG_EXTENDED );
	m_globals->i_hangRetracted = m_joystickDriver2->GetRawButton( HANG_RETRACTED );
	m_globals->i_hangSafetySwitch = m_joystickOperator1->GetRawButton( HANG_SAFETY_SWITCH );
	// Hopper
	m_globals->i_hopperSwitch = m_joystickOperator2->GetRawButton( HOPPER_SWITCH );
	m_globals->i_hopperKicker = m_joystickOperator2->GetRawButton( HOPPER_KICKER );
	// Launcher
	m_globals->i_launcherFireButton = m_joystickOperator2->GetRawButton( LAUNCHER_FIRE_BUTTON );
	m_globals->i_launcherPresetSwitch = m_joystickOperator2->GetRawAxis( LAUNCHER_PRESET_SWITCH );
	m_globals->i_launcherManualAngle = m_joystickOperator2->GetRawAxis( LAUNCHER_MANUAL_ANGLE );
	m_globals->i_launcherManualSpeed = m_joystickOperator2->GetRawAxis( LAUNCHER_MANUAL_SPEED );
	// Other Stuff
	m_globals->i_visionControlCalibration = m_joystickOperator2->GetRawButton( VISION_CONTROL_CALIBRATION );
	m_globals->i_bigRedSafetyButton = m_joystickOperator2->GetRawButton( BIG_RED_SAFETY_BUTTON );
	
	// RECEIVE DATA FROM NETWORK
#endif
}
void Main::Process()
{
#if 0
	// DRIVE TRAIN
	if ( m_globals->i_joystickArcadeToggle )
		if ( fabs(m_globals->i_joystick_2_x) > ARCADE_DEAD_BAND && fabs(m_globals->i_joystick_2_y) > ARCADE_DEAD_BAND )
		{
			m_globals->o_drivetrainLeft = ( m_globals->i_joystick_2_y / DT_NORMALIZATION_CONST ) + ( m_globals->i_joystick_2_x / DT_NORMALIZATION_CONST );
			m_globals->o_drivetrainRight = ( m_globals->i_joystick_2_y / DT_NORMALIZATION_CONST ) - ( m_globals->i_joystick_2_x / DT_NORMALIZATION_CONST );
		}
	else
	{
		m_globals->o_drivetrainLeft = m_globals->i_joystick_1_y / DT_NORMALIZATION_CONST;
		m_globals->o_drivetrainRight = m_globals->i_joystick_2_y / DT_NORMALIZATION_CONST;
	}
	
	// SPATULA
	if ( !( m_globals->i_spatulaUp ^ m_globals->i_spatulaDown )  )
		m_globals->o_spatula = m_globals->o_spatula;
	else
		if ( m_globals->i_spatulaUp )
			m_globals->o_spatula = true;
		else
			m_globals->o_spatula = false;
	
	
	// HANG
	if ( m_joystickOperator1->GetRawAxis( HANG_SAFETY_SWITCH ) == HANG_SAFETY_ON )
		if ( !( m_globals->i_hangExtended ^ m_globals->i_hangRetracted ) )
			m_globals->o_hang = m_globals->o_hang;
		else
			if ( m_globals->i_hangExtended )
				m_globals->o_hang = true;
			else
				m_globals->o_hang = false;
	
	
	// When the safety button is activated, all manual inputs will be ignored,
	// and a series of launch automation sequence will be followed.
	if ( m_globals->i_bigRedSafetyButton && m_globals->i_launcherFireButton )
	{
		// Vision acquires target
		// ********** DEPENDS ON VISION TEAM **********
		
		// Hopper is lifted
		// Discs are lifted within the hopper
		m_globals->o_hopperContainer = true;
		m_globals->o_hopperDisc = true;
		m_globals->o_hopperKicker = true;
		
		// Vision aims the robot, sets the tilt angle and wheel speed
		// Kicker automatically fires upon confirmation from Vision
		// ********* DEPENDS ON VISION TEAM **********
	}
	else
	{
	
		// HOPPER
		m_globals->o_hopperContainer = m_globals->i_hopperSwitch;
		m_globals->o_hopperDisc = m_globals->i_hopperSwitch;
		m_globals->o_hopperKicker = m_globals->i_hopperKicker;
	
		// LAUNCHER
		m_globals->o_launcherLaunch = m_globals->i_launcherFireButton;
		if ( m_globals->i_launcherFireButton )
		{
			if ( m_globals->i_launcherPresetSwitch == 0 )
			{
				m_globals->o_launcherSpeed = LAUNCHER_PRESET_1_SPEED;
				m_globals->o_launcherTilt = LAUNCHER_PRESET_1_TILT;
			}
			else if ( m_globals->i_launcherPresetSwitch == 1 )
			{
				m_globals->o_launcherSpeed = LAUNCHER_PRESET_2_SPEED;
				m_globals->o_launcherTilt = LAUNCHER_PRESET_2_TILT;
			}
			else
			{
				m_globals->o_launcherSpeed = m_globals->i_launcherManualSpeed;
				m_globals->o_launcherTilt = m_globals->i_launcherManualAngle;
			}
		}
	}
#endif
}

void Main::Output()
{
#if 0
	// Drive train actuators
	m_DTLeftA->SetSpeed( m_globals->o_drivetrainLeft );
	m_DTLeftB->SetSpeed( m_globals->o_drivetrainLeft );
	m_DTRightA->SetSpeed( m_globals->o_drivetrainRight );
	m_DTRightB->SetSpeed( m_globals->o_drivetrainRight );
	
	// Spatula actuator
	m_spatula->Set( m_globals->o_spatula );
	
	// Hang actuators
	m_hangA->Set( m_globals->o_hang );
	m_hangB->Set( m_globals->o_hang );
	
	// Hopper actuators
	while ( m_globals->i_hopperContainerState != m_globals->o_hopperContainer )
		m_hopperContainer->Set( m_globals->o_hopperContainer );
	while ( m_globals->i_hopperDiscState != m_globals->o_hopperDisc )
		m_hopperDisc->Set( m_globals->o_hopperDisc );
	m_hopperKicker->Set( m_globals->o_hopperKicker );
	// Launcher actuators
	m_launcherLaunch->SetSpeed( m_globals->o_launcherLaunch );
	m_launcherTilt->SetSpeed( m_globals->o_launcherTilt );
#endif
}

void Main::MidAutonomous()
{
	
}
void Main::SevenAutonomous()
{
	
}
START_ROBOT_CLASS(Main);
